#include "bsp_psw.h"

char *psw_char[] = {"  UP_LF_PSW: ", "  UP_RF_PSW: ", "  UP_LB_PSW: ", "  UP_RB_PSW: ",
                    "  DOWN_F_PSW: ", "  DOWN_B_PSW: ", "  DOWN_L_PSW: ", "  DOWN_R_PSW: "};

int psw_io[] = {UP_LF_PSW, UP_RF_PSW, UP_LB_PSW, UP_RB_PSW,
                DOWN_F_PSW, DOWN_B_PSW, DOWN_L_PSW, DOWN_R_PSW};

uint8_t mode_psw[8] = {0};

/**
 * @brief 初始化PSW
 * @param void
 * @return void
 * */
void psw_Init()
{
    for(int i =0; i<8;i++)
    {
        pinMode(psw_io[i],INPUT);
    }
}
/**
 * @brief 打印所有光电开关的电平状态 检测到东西为1 未检测到为0
 * @param void
 * @return void
 * */
void printPsw()
{

    for (int i = 0; i < 8; i++)
    {
        Serial.print(psw_char[i]);
        Serial.print(digitalRead(psw_io[i]));
        if((i+1) % 4 == 0)
        Serial.println();
    }
}
/**
 * @brief 获取光电开关的电平状态 检测到东西为1 未检测到为0
 * @param uint8_t choice
 * @return void
 * */
void getPsw(uint8_t *pMem, uint8_t choice)
{
    uint8_t current = 0;

    choice_psw(choice);
    
    for (int i = 0; i < 8; i++)
    {
        if(mode_psw[i])
        {
            pMem[current] = digitalRead(psw_io[i]);
            current++;
        }
    }

}

/**
 * @brief 选择要操作的PSW
 * @param uint8_t choice
 * @return void
 * */
void choice_psw(uint8_t choice)
{
    for (int i = 0; i < 8; i++)
    {
        mode_psw[i] = (choice & (1 << i)) >> i;
    }
}
